/*
 * usb_task.c
 *
 *  Created on: 08.06.2010
 *      Author: Phil
 */

#include "usb_task.h"
#include <board.h>
#include <aic/aic.h>
#include "irqproxy.h"
#include <pio/pio.h>
#include "pio_it_rtos.h"
#include <usb/device/cdc-serial/CDCDSerialDriver.h>
#include <usb/device/cdc-serial/CDCDSerialDriverDescriptors.h>
#include <utility/trace.h>
#include <utility/led.h>
#include <pmc/pmc.h>
#include <FreeRTOS.h>
#include <task.h>
#include "ser_proto.h"

#define usbTASK_PRIORITY					( tskIDLE_PRIORITY + 2 )
#define usbBUFFER_SIZE                      64
#define usbPACKET_SIZE                      16
#define usbQUEUE_SIZE                       4

#define USB_EVENT_VBUS      0
#define USB_EVENT_USD_IRQ   1
#define USB_EVENT_SEND      2

typedef struct {
	unsigned char packet[16];
	unsigned char packet_len;
} USBQueueItem;

static xTaskHandle usbTaskHandle;

/// State of USB, for suspend and resume
static unsigned char USBEventFlags;

static unsigned char USBReadBuffer[usbBUFFER_SIZE];

static unsigned char USBWriteBuffer[usbBUFFER_SIZE];
static int           USBWriteLen;

static unsigned char USBReadPacket[usbPACKET_SIZE];

static USBQueueItem USBWritePacketQueue[usbQUEUE_SIZE];
static unsigned char USBWriteQueuePos;
static unsigned char USBWriteQueueLen;


static SerProtoDecodeState USBSerState;

/// List of pins that must be configured for use by the application.
static const Pin pins[] = {PIN_USART0_TXD, PIN_USART0_RXD};

#define FLAGS_SET(dest,flags,mask) ( ((dest) = (flags) ^ (mask)) ,((flags) |= (mask)) )
#define FLAGS_GET_AND_RESET(dest, flags, mask) ( ((dest) = (flags) & (mask)), ((flags) &= ~(mask)) )

// Local function prototypes
static void UsbRecievePacket(unsigned char *data, unsigned int size);
static void UsbDataReceived(void *pArg, unsigned char status,
		unsigned int transferred, unsigned int remaining);
static void UsbDataSent(void *pArg, unsigned char status,
		unsigned int transferred, unsigned int remaining);
static void vUsbTask(void *pvParameters);

//------------------------------------------------------------------------------
//         VBus monitoring (optional)
//------------------------------------------------------------------------------
#if defined(PIN_USB_VBUS)

#define VBUS_CONFIGURE()  VBus_Configure()

/// VBus pin instance.
static const Pin pinVbus = PIN_USB_VBUS;

//------------------------------------------------------------------------------
/// Handles interrupts coming from PIO controllers.
//------------------------------------------------------------------------------
static void ISR_Vbus(const Pin *pPin)
{
	unsigned char is_need_resume;

	FLAGS_SET(is_need_resume, USBEventFlags, (1<<USB_EVENT_VBUS));
	if(is_need_resume)
	{
		xTaskResumeFromISR(usbTaskHandle);
	}
}

//------------------------------------------------------------------------------
/// Configures the VBus pin to trigger an interrupt when the level on that pin
/// changes.
//------------------------------------------------------------------------------
static void VBus_Configure( void )
{
    TRACE_INFO("VBus configuration\n\r");

    // Configure PIO
    PIO_Configure(&pinVbus, 1);
    PIO_ConfigureIt(&pinVbus, ISR_Vbus);
}

#else
    #define VBUS_CONFIGURE()    USBD_Connect()
#endif //#if defined(PIN_USB_VBUS)


//------------------------------------------------------------------------------
//         Callbacks re-implementation
//------------------------------------------------------------------------------
void USBDCallbacks_Initialized(void)
{
	USBEventFlags = 0;
	VBUS_CONFIGURE();

    // Configure and enable the UDP interrupt
    AIC_ConfigureIT(AT91C_ID_UDP, 0, vIRQProxy);
}

//------------------------------------------------------------------------------
/// Invoked when the USB device leaves the Suspended state. By default,
/// configures the LEDs.
//------------------------------------------------------------------------------
void USBDCallbacks_Resumed(void)
{
	// TODO
}

//------------------------------------------------------------------------------
/// Invoked when the USB device gets suspended. By default, turns off all LEDs.
//------------------------------------------------------------------------------
void USBDCallbacks_Suspended(void)
{
	// TODO
}

//------------------------------------------------------------------------------
/// Indicates that the current configuration of the device has changed.
/// \param cfgnum  New device configuration index.
//------------------------------------------------------------------------------
void USBDDriverCallbacks_ConfigurationChanged(unsigned char cfgnum)
{
	if(cfgnum != 0)
	{
		// reset send queue
		taskENTER_CRITICAL();
		USBWriteLen = 0;
		USBWriteQueuePos = 0;
		USBWriteQueueLen = 0;
		taskEXIT_CRITICAL();

		// reset packet decoder
//		SerProto_DecodeInit(&USBSerState, USBReadPacket, usbPACKET_SIZE);

		// read data from host
//		CDCDSerialDriver_Read(USBReadBuffer, usbBUFFER_SIZE, UsbDataReceived, 0);
	}
}

static void UsbRecievePacket(unsigned char *data, unsigned int size)
{
	// TODO
}
//
static void UsbDataReceived(void *pArg, unsigned char status,
		unsigned int transferred, unsigned int remaining)
{
	// Check that data has been received successfully
	if (status == USBD_STATUS_SUCCESS) {

		// Decode packet from serial stream
		unsigned int pos = 0;
		while(pos < transferred) {
			pos = SerProto_Decode(&USBSerState, USBReadBuffer, pos, transferred);

			// is decode finished?
			if(SerProto_DecodeLength(&USBSerState) != -1)
			{
				UsbRecievePacket(USBReadPacket, (unsigned int)SerProto_DecodeLength(&USBSerState));

				// reset packet decoder
//				SerProto_DecodeInit(&USBSerState, USBReadPacket, usbPACKET_SIZE);
			}
		}

		if((transferred == usbBUFFER_SIZE) && (remaining > 0)) {
			// same bytes dropped - reset packet decoder

//			SerProto_DecodeInit(&USBSerState, USBReadPacket, usbPACKET_SIZE);
		}
	}
}

static void UsbDataSent(void *pArg, unsigned char status,
		unsigned int transferred, unsigned int remaining)
{
	// TODO
	taskENTER_CRITICAL();
	USBWriteLen = 0;
	taskEXIT_CRITICAL();
}

static void vUsbTask(void *pvParameters)
{
	TRACE_INFO("USB task start\n\r");

	PIO_EnableIt(&pinVbus);
	AIC_EnableIT(AT91C_ID_UDP);

#if defined(PIN_USB_VBUS)
	// Check current level on VBus
	if (PIO_Get(&pinVbus)) {

		// if VBUS present, force the connect
		TRACE_INFO("VBUS conn1\n\r");
		USBD_Connect();
	}
	else {
		TRACE_INFO("VBUS discon1\n\r");
		USBD_Disconnect();
	}
#else
	USBD_Connect();
#endif

	for(;;)
	{
		unsigned char flags = 0;
		taskENTER_CRITICAL();
		do {

			FLAGS_GET_AND_RESET(flags, USBEventFlags, ((1 << USB_EVENT_VBUS) | (1 << USB_EVENT_USD_IRQ)));
			if(!flags) {
				if(USBWriteQueueLen != 0 && USBWriteLen == 0) {
					flags |= (1 << USB_EVENT_SEND);
				}
			}
			if(!flags) {
				vTaskSuspend(NULL);
			}
		} while(!flags);
		taskEXIT_CRITICAL();

		if(flags & (1<<USB_EVENT_VBUS))
		{
		    // Check current level on VBus
		    if (PIO_Get(&pinVbus))
		    {
		        TRACE_INFO("VBUS conn2\n\r");
		        USBD_Connect();
		    }
		    else {

		        TRACE_INFO("VBUS discon2\n\r");
		        USBD_Disconnect();
		    }
		}

		if(flags & (1<<USB_EVENT_USD_IRQ))
		{
			// service USD interrupt
			USBD_InterruptHandler();

			// re-enable USD interrupt in AIC
			AIC_EnableIT(AT91C_ID_UDP);
		}

		if(flags & (1<<USB_EVENT_SEND))
		{
			USBQueueItem *item;

			taskENTER_CRITICAL();
			{
				// dequeue & encode packet
				item = &USBWritePacketQueue[USBWriteQueuePos];
				USBWriteLen = SerProto_Encode(USBWriteBuffer, usbBUFFER_SIZE, item->packet, item->packet_len);
				USBWriteQueuePos = (USBWriteQueuePos + 1) % usbQUEUE_SIZE;
				USBWriteQueueLen--;
			}
			taskEXIT_CRITICAL();
			CDCDSerialDriver_Write(USBWriteBuffer, USBWriteLen, (TransferCallback)UsbDataSent, 0);
		}

//		// Device is not configured
//		if (USBD_GetState() < USBD_STATE_CONFIGURED) {
//
//			// Connect pull-up, wait for configuration
//			USBD_Connect();
//			while (USBD_GetState() < USBD_STATE_CONFIGURED);
//
//			// Start receiving data on the USB
//			CDCDSerialDriver_Read(usbBuffer,
//								  DATABUFFERSIZE,
//								  (TransferCallback) UsbDataReceived,
//								  0);
//		}
//		if( USBState == STATE_SUSPEND ) {
//			TRACE_DEBUG("suspend  !\n\r");
//			//LowPowerMode();
//			USBState = STATE_IDLE;
//		}
//		if( USBState == STATE_RESUME ) {
//			// Return in normal MODE
//			TRACE_DEBUG("resume !\n\r");
//			//NormalPowerMode();
//			USBState = STATE_IDLE;
//		}
	}
}

void USBTask_InterruptHandler()
{
	unsigned char is_need_resume;

	AIC_DisableIT(AT91C_ID_UDP);

	FLAGS_SET(is_need_resume, USBEventFlags, (1<<USB_EVENT_USD_IRQ));
	if(is_need_resume)
	{
		xTaskResumeFromISR(usbTaskHandle);
	}
}

void USBTask_Init(void)
{
	USBWriteQueuePos = 0;
	USBWriteQueueLen = 0;
	USBWriteLen = 0;

	// driver initialization
	CDCDSerialDriver_Initialize();

	xTaskCreate( vUsbTask, (const signed char * const)"USB", configMINIMAL_STACK_SIZE + 50, NULL, usbTASK_PRIORITY, &usbTaskHandle);
}

int USBTask_Send(const char *data, int size) {
	int result;
	taskENTER_CRITICAL();
	if(USBWriteQueueLen < usbQUEUE_SIZE) {
		int i;
		USBQueueItem *item = &USBWritePacketQueue[(USBWriteQueuePos + USBWriteQueueLen) % usbQUEUE_SIZE];
		item->packet_len = size;
		for(i = 0; (i < size) && (i < usbPACKET_SIZE); ++i) {
			item->packet[i] = data[i];
		}
		USBWriteQueueLen++;
		result = -1;
	} else {
		result = 0;
	}
	taskEXIT_CRITICAL();
	vTaskResume(usbTaskHandle);
	return result;
}
